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ABSTRACT 


A truly Autonomous Vehicle must be able to determine its global position in the 
absence of external transmitting devices. This requires the optimal integration of all 
available organic vehicle attitude and velocity sensors. This thesis investigates the 
extended Kalman filtering method to merge asynchronous heading, heading rate, 
velocity, and DGPS information to produce a single state vector. Different 
complexities of Kalman filters, with biases and currents, are investigated with data 
from Florida Atlantic's Ocean Voyager II surface run. This thesis used a simulated 
loss of DGPS data to represent the vehicle's submergence. All levels of complexity of 
the Kalman filters are shown to be much more accurate then the basic dead reckoning 


solution commonly used aboard autonomous underwater vehicles. 
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updates from outside the ocean environment. 





I. INTRODUCTION 
A. BACKGROUND 
There are numerous applications where the usage of an Autonomous 
Underwater Vehicle (AUV) is desired. Exercises in ocean floor mapping, under ice oil 
exploration, mine field mapping and clearance all require the vehicle to operate with 
little or no outside assistance from the ocean environment. A primary demand upon 


the vehicle is that it be able to determine its absolute location relative to a global 


reference system and maintain an update on that position with infrequent position 


eee ee as ie pileaieiniens aed for small 
AUVs must satisfy the constraints of the AUV itself: small size, low power 
consumption, and be inexpensive. 

When the AUV is surfaced, accurate position updates can be obtained with the 
Differential Global Positioning System (GPS). This system is commercially available, 
accurate, inexpensive, and small. However, the high frequency waves transmitted 
from the GPS satellites can not travel underwater [Ref. 1: p.2]. Large submersibles 
currently use a form of Inertial Navigation System (INS). The INS system is not 
suitable for AUVs because of its tremendous cost and bulky size [Ref. 2]. Thus, while 
the AUV is submerged and operating without any information input from above the 
surface (i.e. DGPS), its position must be obtained by integrating sensors that measure 
the different aspects of the navigation problem. In keeping the cost down, the 
complexity of the velocity and heading sensors must also be kept low. With lower cost 
sensors, the amount of error and bias increase. The heading sensors could range from 


] 





sensors, the amount of error and bias increase. The heading sensors could range from 
a simple magnetic compass to a laser gyro, which also provides heading rate. Velocity 
sensors vary from the most inexpensive pitot tube to Doppler sonar. In order to 
maintain an accurate position in the presence of currents, the velocity sensor must 
provide speed relative to the ocean floor and not relative to the water. Measuring the 
AUVs speed relative to water causes excessive error from the uncertainties of the 


currents [Ref. 2]. 


B. CURRENT NAVIGATION METHODS 

The most basic navigation method is to measure heading and multiply measured 
velocity by a fixed interval of time to get distance traveled from a known initial 
position. This method, dead reckoning, has been used with vehicles with no inertial 
sensors or velocity sensors to determine position and velocity with respect to the ocean 
floor [Ref. 3]. These vehicles were used in missions with short endurance times 
(minutes) and short ranges (less then 10 nautical miles) such as torpedoes [Ref. 3]. 
Any bias and random error in the velocity measurement are also integrated and cause 
errors in position calculation which grow during extended periods between DGPS 
updates [Ref. 3]. Incorporating these errors and sensor bias into the integration 
routine would provide a more accurate position. 

The recursive method for integrated the data from the different sensors, 
velocity, heading, and when obtained DGPS update, is the Kalman filter [Ref. 2]. The 
Kalman filter performs three major functions, optimally integrates data from multiple 
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sensors, incorporates models of the sensors error characteristics, and recursively 
processes the measurements from the sensors that are available [Ref. 3]. The 
complexity of the Kalman filter resides in how many states (i.e. north and south 
position, velocity, possible bias, and heading) are taken into account in the filter 
routine. With added states, any information about one state or set of states will be used 
to improve system performance [Ref. 2]. In the Kalman filter if the different sensors 
have small random errors (i.e. accurate measuring device), then the result of the filter, 
the filter states, will be more accurate. The most important sensor in the integration 
process is the velocity sensor. 

The system that will be analysized in this thesis was built by Florida Atlantic 
University Oceanic Engineering department. The navigation system is comprised of a 
Watson IMU (inertial Motion Unit), RD Instruments doppler velocity log (Acoustic 
Doppler Current Profiler), Motorola GPS receiver, and an Acupoint DGPS receiver. 
The IMU measures angles in azimuth (true heading), pitch, and roll and computes the 
angular change rate for these angles. 

The ADCP measures velocity relative to a column of water in all three 
orthoganal directions. These measurements are then converted to the global reference 
system, north, east, and down (toward center of earth). Since these measurements are 
relative to a column of water, they include the current. In order to get a true vehicle 
velocity over ground without current, a bottom tracking measurement is obtained. 
However, the bottom must be within range of the sensor in order for bottom tracking to 


occur. 





When the vehicle is at the surface, both DGPS and GPS might be available for 
accurate position updates. The vehicle will use the data from the DGPS since it is 
more accurate than the GPS system. Commercial GPS is only accurate to 
approximately 54.9 meters RMS and DGPS is accurate to within 2 centimeters [Ref. 6] 
depending on the type of correction used. This could be a source of possible problems 
because if data is obtained from DGPS and then the next data point is from GPS there 


will be large errors introduced into the Kalman Filter. 


c. THESIS SCOPE 

Autonomous Underwater Vehicle localization is often achieved by integration of 
internal state information (velocity, heading, and heading rate) with external 
measurements (DGPS or acoustic beacons) [Ref. 4]. The focus of this thesis is to 
explore an alternative navigation system utilizing internal measurements only instead of 
those methods mentioned above. The system analysized in this thesis is the Ocean 
Voyager sponsored by Florida Atlantic University, discussed in the previous section. 
There will be three levels of complexity in Kalman filters discussed, six state, nine 
State, and ten state. The six state does not include any bias states or current 
corrections. Bias states for the heading and both doppler velocities are included in the 
nine state filter. In the ten state doppler velocity states are exchanged for relative 
velocities and current state along with bias states for heading and heading rate. The 
thesis 1s organized in the following manner; Chapter I is an introductory Chapter with a 
general background of the navigation problem and objective of this thesis. Chapter II 
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is the development of the different levels of complexities of the Kalman filter used in 
the solution to the navigation problem presented in this thesis including the 
programming algorithm. Chapter II analyzes the difference between a synchronous 
discrete Kalman filter algorithm and the asynchronous algorithm. Chapter IV will 
analyze the results with and without a loss of GPS updates. Conclusions and 


recommendations for further research are provided in Chapter V. 

















Il. KALMAN FILTERING 

A. INTRODUCTION 

Kalman filtering is the process of recursively updating an estimate of system 
state based upon measurements corrupted by noise. The system state is a collection of 
variables that describe inertial dynamics of a system, in this case those variables 
include global position, velocity, heading, heading rate, and sensor bias. A system 
state vector for the vehicle can include all or more or less then those mentioned. Some 
of these states will not be able to be measured directly, such as when the vehicle goes 
under water and DGPS ata are not available. System states are updated with 
knowledge of system dynamics (system model), measurement dynamics (measurement 
model) system noises, measurement noises, and initial conditions for the system state 
[Ref. 7]. The system model is not perfect in describing the dynamics of the vehicle 
and will contain a certain amount of uncertainty, this is called the system noise. There 
is also a certain amount of uncertainty associated with each measurement taken. This 
uncertainty is composed of random white noise and a bias. These uncertainties in the 
measurements are included in the measurement model. Measurements which can not 
be directly obtained, such as global vehicle velocity, when no bottom tracking is 
available, are related to measurements which are directly obtainable, such as vehicle 
velocity relative to a water column, in the measurement model or C matrix. 
'Recursively updating’ means that the Kalman filter does not need to keep a record of 
all past measurements, only the most recent one. In this thesis, the change in the 


system state during a set interval of time will be the state vector used in the Kalman 
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filtering process, thus the Kalman filters in this thesis follow the general form: 
X(N-fX(),)-O(). Where f(X(t),t) is a nonlinear system equations describing the vehicle 
and Q(t) is a constant white noise with a zero mean associated with the system. The 
linearized form of f(X(t),t) is A. The system and measurement models for the six, 


nine, and ten state filtered are developed in the following sections. 


B. MODEL DEVELOPMENT 
The system model for the different complex Kalman filters are based on the 
general form of the simplest navigation method, dead reckoning: 


X=X(t,)XeAt 
Y-¥(t,)+YeAt (2.1) 


Where X(t.) and Y(t,) is the last longitude and latitude position respectively of the 
vehicle and At is the time interval between updates to X and Y. The vehicles global 
velocity is given by: X-dop,-cos(y)-dop,-sin(y)and Y-dop -sin()-dop -cos(y) where dop, and 
dop,, are the velocities of the vehicle relative to the ground in body centered coordinates 
and w is the global heading. Using the basic form of the Kalman filter described in the 
previous section, updates to the state vector, X(t), is accomplished through 
X(t)-X(t,)-X()-At where X(t,) is the initial values of the state vector, At is a fixed time 
interval, and X(#)is the change in the state vector over the time interval. The simplest 


Kalman filter studied is the six state model which is discussed in the following section. 





1. Six State System Model 
The six state system mide includes the basic equations describing dead 
reckoning of the vehicle, heading, heading rate, and local velocity over ground no bias 
adjustments are including in this model. The system nonlinear equations are as 
follows: 
X-dop »cos(y)-dop »sin(y)+g1 


Y-dop »sin()+dop,+cos(p)+q2 


W-0+q3 
F=0+9q4 


dop -0-g5 ee) 


dop,-0+q 6 


In Eq. 2.2, r is the heading rate and q, are the elements of the noise vector Q 
associated with the corresponding state equation. The equations relating the obtainable 
measurements to the inferred measurements is presented in the next section. 

2. Six State Measurement Model 

The measurements that are directly obtainable are heading, heading rate, 
velocity over ground in vehicle centered coordinates, and position when the vehicle is 
surfaced. Thus all six states, composing the measurement vector Y, are measurable 


when the vehicle is surfaced and only four when the vehicle is submerged. 








The measurement equations are as follows: 


dop -dop +v, 

dop,-dop,+v, (2.3) 
bey-v, 

r=r+V, 

XaXeV , 

Y-¥+v. 


In the measurement model, each measurement has an associated variance, v,, which 
describes the width of the distribution of that particular measurement. Realizing that 
the velocity sensor also has a bias associated with it, the nine state filter includes the 
bias for velocity and heading. 

3. Nine State System Model 
In addition to the equations in equation 2.3, the equations for the change in 


velocity and heading bias are included to arrive at: 


X -dop »cos(y)-dop «sin(p)-q/ 

¥-dop,-sin()-dop -cos(p)+q2 (2.4) 
ip -0-93 

¥-0-94 

dop -0-q5 

dop -0-g6 

b -0-g7 

b -0-98 

b -0-99 


In Eq. 2.4, b,, b,, and b,; are the biases associated with the velocity and heading 


10 





sensors respectively. With these added biases, a more accurate model of the true 
navigational path of the vehicle will be obtained. These biases will be ‘learned’ by the 
Kalman filter before the vehicle submerges and with these learned biases, an accurate 
position of the vehicle will be maintained. The measurement model for the nine state 
filter is altered with the presence of these biases on heading and velocity. 

4. Nine State Measurement Model 

The nine state model incorporates the bias states on velocity and heading and is 


as follows: 


dop -dop +b +v, 
dop,-dop +b +v, 
w = Uy +b es Vv 3 


Y=7+V 4 


eee (2.5) 
Y-Y+v . 


In the preceding models of six and nine states, the currents were estimated to be zero, 
this, of coarse is not the case in a real ocean environment. In the ten state filter 
the currents will be states and used to maintain an accurate position while the vehicle is 


submerged without DGPS fixes. 


I] 











5. Ten State System Model 
Adding the currents to Eq 2.4 results in the following system of equations: 
X=u -cos(p)-v -sin(y)-u_+g, 
Y-u, «sin(p)-v,cos()-u_+g, 
Wereg, 
u-0-q, 
v -0+9, 


U..-0°4, (2.6) 
u,-0 *q. 

r-O-g, 

b -0-q, 

b,-0-4,, 


It should be noted that the currents, u,, and u,, are in the global reference frame and the 
vehicle velocities relative to the water column, u, and v, are in the vehicle centered 
reference frame and are rotated to the global reference frame. 

6. Ten State Measurement Model 

In Eq. 2.6, b, is the bias associated with the heading rate and b,, is the bias 
associated with the heading compass. The measurement model in the ten state filter is 
similar to Eq. 2.5 with the replacement of dop, and dop, with u, and v, since the 


currents are to be added to the relative velocities to obtain global velocity. 


LZ 





Altering equation 2.5 for the ten state measurement model results in the following 


system of equations: 


dop =u_»cos()+u y*Sin(y)-u,+Vv , 

dop,-u,,*Sin()+u, «cos(P)+v,+V, 

UU HV, 

-W+bv, 

r=r+b +V, 

Vevey, (2.7) 
X=X+V 7 

Y=Y+v 8 


As in the measurement model in Eq 2.7, v, is the associated variance with the 
measurement sensor x. A Kalman filter routine was written for six, nine, and ten state 
filters. The next section explains how this algorithm is developed from the nonlinear 


mathematical equations presented in this section. 


C. KALMAN FILTER ALGORITHMS 

The algorithm used in the MATLAB code for all three Kalman filters, six, nine, 
and ten state, is based on the same procedure. First the system model matrix A, 
system noise matrix Q, measurement model C, measurement noise matrix R, and the 
error covariance matrix P are initialized to appropriate initial values (.e., longitude, 
latitude, and heading). The error covariance matrix is a measurement in the 
uncertainty in the state vector X. Then the state vector, error covariance, and 
measurement vector are propagated one time step. When the new measurement is 


received, an innovation error is calculated. With the propagated error covariance, 
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measurement noise, and measurement model, a gain is determined for the state vector 
and error covariance update. This process of propagation and updating or corrected is 
repeated until the end of the vehicle mission. Following the timing diagram in Figure 


1{Ref. 8], the following variables are defined for the Kalman filter. 


H.R: 
Hi. Re. A 
A Xi) XC) 
I= Xi.) 
OD ss Qe 
Pri(-) Pra(+) Pi(-) | Ps(+) 


—— -- a a 


tk ti 


Figure 1 Discrete Kalman Filter Timing Diagram 


Where: 
X,;.,(+): Initial state vector 
P,.,(+): Initial covariance matrix 
®,., : initial transition matrix = eA* 
Q,.;: initial system white noise with zero mean, remains constant, = @ 
H,..,: initial linearized measurement matrix, C,., 
R,., : measurement noise, remains constant, = v* 
X,(-) : propagated state vector 
P,(-) : propagated covariance matrix 
H,. : updated measurement matrix after measurement received, C, 
R, : measurement noise = R, , 
A,(+) : State vector updated after measurement received 
P,(+): updated covariance matrix after measurement received 
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The basic theory behind a Kalman filter is to find some value that minimizes the 
error between the estimated X and the actual value of X. Since P =E[XX°], the result 
is a symmetric matrix with the diagonal terms being the mean square error of the state 
variables with themselves and the off diagonal terms the cross-correlation of the state 
variables. The cross-correlation between the state variables is zero thus only the 
diagonal of P provides a measurement of the difference between the estimated state 
vector and the actual value of the state vector. The value that minimizes the trace(P) or 


the sum of the diagonal elements is the Kalman gain or K. It is defined as [Ref. 8]: 


K,-P (-)-H, (HP (OH, Ry" (2.8) 


The values for P are initially set to about 1.7. In the actual MATLAB code, the 
procedure follows the above timing diagram and is the same for the six, nine, and ten 
state filters, ekf fau6.m, ekf_fau9.m, and ekf_fau6.m. Since the filter is based upon a 
propagate and correct sequence, if there is a large change between successive 
measurements, the filter will try to compensate and create large gains. These large 
gains will cause other states to be erroneously propagated causing large innovation 
errors which will cause the filter to possibly go unstable. This is highly undesirable in 
the case of the heading variable when the heading crosses over the 360°/000° point. In 
this case instead of resetting the heading back to 000°, it is continues increasing beyond 
360°. This also enables counting how many time the vehicle turns around completely 


by dividing the final heading in the mission by 360°. This procedure is done prior to 
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the initialization for A, Q, C, R, and P. After the initialization, the system 
transition matrix is formed, ®, to be used in propagating the error covarance matrix, 
P. Next the state vector is propagated in the prop6.m, prop9.m, and prop.m functions 
by the simple procedure of multiplying eq 2.2, 2.4, and 2.6 for the six, nine, and ten 
State filters respectively, without the system noise q by the time step, At. Then 


propagation of P is performed by the following in accordance with Figure 1 [Ref. 8]: 


P(-)-®, =P, (+), +O (2.9) 


The last propagation to be done before the measurement data is taken is for the 
measurement vector Y. This is done in the output6.m, output9.m, and output.m 
functions for the six, nine, and ten state filters. The procedure is multiplying eqs 2.3, 
2.5, and 2.7 without the measurement noise, v, by At. With the reception of the 
measurement data, the innovation error 1s formed by taking the difference between the 
propagated Y and the new measurement Y. If the any of the values for the estimated Y 
are the same as the measured Y, then no new data was received for that variable. The 
corresponding multiplier in the C matrix is set to zero causing no change in that state 


variable after the gain is applied and correction made to the propagated state variable. 
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The Kalman gain, K, as determined in eq. 2.8 is used to update the covariance 
matrix, P,(-) to P,(+) by the following [Ref. 8]: 
(2.10) 
P A+)=L-K+, J» ‘C-) 


I is the identity matrix 


In each of the filter codes, the diagonals of P are saved for each time step to be 


analyzed after the mission. The final step in updating the state vector is: 


eee XKeerr =. 
err is innovation error (2.11) 


With the state vector and the error covariance matrix updated, the matrices A and C 

are evaluated with the new values from the state vector. For comparison in each filter 
routine, the dead reckoning solution from eq. 21 is evaluated from the initial values 
and compared to the filtered solution. The actual MATLAB code for the main Kalman 
filter routines: ekf fau_6.m, ekf fau_9.m, and ekf_fau_2.m, are contained in Appendix 
A. The state vector propagation functions: prop6.m, prop9.m, and prop10.m are 
contained in Appendix B. Measurement vector propagation functions output6.m, 


“> “oiifput9.m, and outputl0.m are contained in AppendixC. 
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III. ASYNCHRONOUS DATA PROCESSING 
A. ASYNCHRONOUS DATA PROBLEM 
In the Kalman filter presented above with the timing diagram of Figure 1, all data 
was assumed to be received at the same time at an equal time interval throughout the 
mission. In reality, all the measurements in the Y vector are not received at each time 
they are requested. Even from one update to the next, measurements received at one 
update may not be renewed in the next update. Figure 2 will be used in further 


description of the problem and the solution employed. 





Time Step 
Figure 2 


In Figure 2, new data for a sensor is received where there is a change in amplitude 
from one time step to the next. Data for a sensor was repeated where the amplitude 
remains constant from one time step to the next. For example, dop_u is updated at tp, 
t,, ty, and t, but not between those times. The longitude and latitude recieve new 
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values only at time t) from Figure 2. This process could have taken place for variable 
in the measurement vector. Missing data will cause serious problems for the Kalman 
filter developed thus far because any missing data would have been treated as a zero 
value. This zero value would mean to the filter the sensor was reading a value of zero 
instead of simply no new data for that sensor. For example, if the heading sensor at 
time t, was not new, a zero would normally be sent to the filter. The filter would then 
interpret this as the heading is zero at time t, instead of the truth that the heading has 
not changed from what it was at time t, The data that is used for the six, nine, and 
ten state filters in this thesis contain instances where not every measurement is updated 
at the same frequency. Two questions must be answered to solve the asynchronous 
data problem. First, each sensor runs at a different speed and second the Kalman filter 
process must run fast enough to complete its process before the next update from the 
sensors is available. The different sensors that are on the Ocean Voyager II run at the 
following frequencies: DGPS runs at 1Hz, doppler sonar runs at 2Hz, and the compass 
is sampled eight times every second. In order to minimize lost data, the filter must 
run at a frequency that is faster than the fastest sensor. Even at this if the filter 
operates at this rate, at regular interval time steps, all the sensors may not have 
information. as in Figure 2. A specific problem still arises after there is pre-processed 
data for every time step. How does the Kalman filter treat the covariance and gain of 
this repeated measurement? If the data is repeated (i.e. unchanged from t to t+dt) then 
there should be no correction to that element in the state vector and the corresponding 
element in the covariance matrix. The next section describes how the MATLAB 
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Kalman filter codes were adjusted to overcome the problems of old data and correction 
of gains and the covariances across the measurement from time ,,,(-) to 4; (+) 


according to the timing diagram of Figure 1. 


B. ASYNCHRONOUS DATA SOLUTION 

The data used in this thesis came from the mission file from surface run on Aug 24, 
1996. This mission file only contains data updates for all the navigation sensors, not a 
continous data file for the entire time of the mission. Thus, before the data is used by 
the Klaman filter, it is ‘filled’ by repeated old data between updates, this has the effect 
of a zero order hold. This was shown in Figure 2 for the case where longitude ,X, 
and latitude, Y, was not available from time step t,. This way before the data gets 
processed by the Kalman filter equations as discussed in Chapter II there is data at each 
time step throughout the mission. Where the data is repeated, there should ideally be 
no correction to that data variable in the state variable from equation 2.11. Thus the 
gain for the variable that was repeated should be zero. At the same time there should 
be no correction to the covariance matrix element corresponding to the repeated data 
variable. This is done through the measurement model by setting those elements in the 
C matrix to zero which, when multiplied with the gain as in eq 2.10, will cause the 
corresponding covariance element in P to also be zero. Since Gi (P,C,,)and if 
there is no update in the latitude and longitude data then C,, and C,, = Q for all k that 
latitude and longitude is repeated. The previous operation for G will cause G;, and G, 


to = 0 for all rows i. When substituting this adjusted gain matrix into eq 2.10, the P,, 
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and P., will be the same as before the measurement. Thus the only update to X will be 
from propagation from one time step to the next without any correction to the elements 
for which no new information is received. This follows the general principle of the 
Kalman filter used for the case with asynchronous data of uncorrected propagation of 
the state vector and covariance matrix between measurements followed by correction 
when measurement data is available. All three filter programs use this principle of 


uncorrected propagation and applied correction across the measurement. 


ZZ 








IV. SIMULATION RESULTS 

A. INTRODUCTION 

The data collected to be processed by the Kalman filters was collected by Florida 
Atlantic University on 27 Aug 1996 from the Ocean Voyager II during a mission that 
was run at the surface off the coast of Florida near Ft. Lauderdale. Since this mission 
was run entirely at the surface there is DGPS/GPS data for the entire run. In order to 
accomplish the goals of this thesis, underwater navigation, the data was pre-processed 
to simulate a majority of the run underwater (no DGPS/GPS data). Simulation of an 
underwater run was iene by not alanis longitude and latitude for the remainder of | 
the mission after a certiain time that 'submergance’ occurred. This has the same effect 
as the vehicle submerging and not recieving any DGPS updates after submergance. 
This was done after the vehicle was allowed to run on the surface for approximately 
8.3 minutes allowing the filter to 'learn' the bias states for the higher order filters. 
Figure 3 shows the actual data run to be used based on DGPS/GPS data. Each filter 
was compared to a dead reckoning solution which was initiated at the beginning of the 
mission. There are several ways in which the Kalman filter can be evaluated, the most 
important in the case of underwater navigation is minimization of radial mean square 
error between the dead reckoning solution and the resulting predicted path from the 
Kalman filter. There exists optimum values for R that will minimize the radial error. 
For each filter, several different increasing multipliers for R were tried and the 
average radial error for each multiplier was plotted against the multiplier in a semilog 


plot for the six, nine, and ten state filters. Figure 4 shows the plot for the six state 
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filter. From this plot, the lowest average radial error is a minimum for a multiplier of 
one. Thus all of the results for the six state filter were produced with the same values 
for R. This process was repeated for the both the nine and ten state filters. In closer 
analysis of each of the filters, the innovation error for the position data will be 
investigated along with the covariance values for position, heading, and ground speed. 
In the higher order filters that include bias as a state variable, these biases will also be 
analyzed to ensure that they steady out to a constant value over the period of the run. 
After all filters are analyzed against the dead reckoning solution they will then be 
compared to each other to investigate how important are the bias states used in the nine 


and ten state filter and the current states used in the ten state filters. 


B. SIX STATE KALMAN FILTER 

The six state Kalman filter as presented in equation 2.2 is the simplest filter and 
does not include currents, assuming them to be negligible, or biases for the heading 
and velocity sensore Figure 5 shows the track of the true data from DGPS/GPS data, 
the dead reckoning solution based on equation 2.1, and the Kalman filter output. From 
the Figure it can be seen that soon after the point where the vehicle simulated going 
underwater, the Kalman filter solution began to slowly deviate from the true track. 
The amount the filter solution varied from the true path varied during the mission. 
However, from the Figure, the filter solution was always better then the dead 
reckoning solution. Since there are no bias states or current states in the six state 
model the deviation from the actual path was expected. 
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Vehicle Path tracked by DGPS on 27 Aug 1997 
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Figure 3: DGPS Track of Vehicle on Surface 
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6 State Average Radial Error 
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Figure 4: Average Radial Error vs R Multiplier 





In the timing diagram of Figure 2, there was just continuous propagation of longitude 
and latitude variables. However, taking a closer look at Figure 5, there is an another 
advantage to even this simple Kalman filter over dead reckoning, smoothing of the 
track during jumps in DGPS updates and movement around turns when DGPS was not 
available. Figure 6 is a zoomed look at the second turn, before the simulated 
submergence. As seen from Figure 6, the DGPS tracking gets irregular right after the 
turn while the Kalman filter does not change to the extremes of the DGPS updates. 
The Kalman filter solution is still very close to the DGPS tracking data because at this 
point in time, the vehicle is still getting DGPS updates thus the complete propagation 
and correction procedure is occurring. After the vehicle submerges and there is no 
longer a DGPS update, the filter still behaves smoothly with little variance. Figure 7 


shows a close up of Figure 5 where the vehicle is submerged. 
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6 State Filter with Dead Reckoning and DGPS 
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Figure 5: DGPS Track, 6 State Kalman Filter, and Dead Reckoning 
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Figure 6: 6 State Filter Smoothing 
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Notice that although the real track (DGPS) varies greatly and irregularly, the filter 
solution smoothly navigates the curves and the straight course. Since the propagation 
model is based on accurate measurements of speed and heading, from equation 2.2, the 
Figures 8, 9 and 10 show the closeness of the Kalman filter prediction of heading and 
forward velocity, doppler_u, as compared to the actual measurements. Figure 9 is a 
close-up view of a segment of Figure 8 showing the smoothness of the filter. As can 
be seen from the area around 875 880 seconds, there is a loss of heading signal and the 
filter keeps tracking —— until the heading information is updated. Also noticeable 
is that as the heading ieee is jumpy through the updates, the filter tracks dale 
and smoothly being fairly reactive to the sensor. This results from the low system 
noise in the q vector and the low measurement noise element in the R matrix 
corresponding to the heading. Each diagonal element corresponds to a specific sensor. 
From equation 2.8, the greater the value of R the lower the value of gain and thus the 
filter will react more slowly and more smoothly. Since the variance in heading 
measurement is less then that shown in longitude and latitude (Figure 5), the 
corresponding R values will be less for heading. Figure 10 shows the variance in 
doppler velocity in the u direction with the filtered doppler u. As can be seen, the 

: Peace nica larger then heading and thus the reason for the-high values in R 
corresponding to velocity causing the filter to ' believe’ the model more then the 
measurement. If the filter was too reactive to the sensor in this case, the filter would 


not be able to provide a smooth prediction of the vehicle path. 
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Figure 8: 6 State Filtered Heading and Measured Heading 
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Figure 9: 6 State Filtered Heading and Measured Heading 
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Figure 10: 6 State Filtered Doppler u and Measured Doppler u 
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With the optimum values for the measurement noise matrix being used in the 
simulation, there is still a difference between the filter track and the actual track as 
shown in Figure 5. Figure 11 shows the innovation error, from equation 2.10, is not 
evenly distributed on either side of zero for longitude or latitude showing the presence 
of a current. For comparison with the nine and ten state filters, the innovation error 
for latitude and longitude has also been plotted separately in Figures 12 and 13. From 
all three Figures it can be seen that innovations for both latitude and longitude change 
dramatically during the entire mission from the pont of submergence. EADS 11 also 
shows that the errors are not hae ie aks but are offset to $m | in latitude and - 
5m in longitude. This offset could be the result of a velocity bias not accounted for. 
Computing a radial error for the six state filter as the magnitude of the difference 
between the measured DGPS track and the filter solution and repeating this process for 
the dead reckoning solution results in Figure 14. The radial error for both the filtered 
solution and the dead reckoning are shown together for comparison. The greatest 
change in the radial error for the filter occurs where the vehicle 'went submerged’. 
The other area of large variation can be attributed to wave action that caused the DGPS 
track to change greatly. As seen from Figures 6 and 7 the filter path is quite smooth. 
From Figure 14 the important result is that for the entire mission the six state filter 
performs better then the dead reckoning solution. When the vehicle goes ‘submerged’, 
the stability of the state estimation error variables is also lost. With an unstable mode 
in the system model, the error covariance for the elements where no measure was taken 
will grow unbounded [Ref. 8]. 
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Figure 11: 6 State Innovation Error in Latitude and Longitude 
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Figure 12: 6 State Innovation Error for Latitude 


sf 





delta longitude (m) 


20 


15 


10 


OV 








6 State Longitude Innovation Error 


500 1000 1500 
time (sec) 


Figure 13: 6 State Innovation Error for Longitude 
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Figure 14: 6 State Radial Error 
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It must be remembered that the design of the Kalman filter is based upon equation 
2.10 and producing a more accurate update to the proagated state vector by applying 
the appropriate Kalman gain to the innovation error. With the innovation error 
increasing rapidly after submergence, the gains will immediately adjust, based upon the 
R matrix, to bring the innovation error down. After simulated submergence, the radial 
error grows large because the latitude and longitude are remaining the same from the 
sensor (submerged) and the propagated measurement is from the filter model causing a 
large innovation error. However, since no updates from DGPS occur for the 
remainder of the ieilie neither the innovation error nor the error covariance can ever 
be reduced to the values it was before the loss of data, there is no correction process 
taking place for the longitude and latitude state variables. In the development of the 
Kalman filter it was assumed that there was no cross correlation between the state 
variables. Figure 15 and 16 show the inaccuracy in that assumption. From Figure 15, 
the error covariance for doppler u changes at the point of ‘submergence’. In the 
simulated submergence there is no change in measured velocity since the vehicle never 
actually submerged. Since there is a definite relationship between position and 
velocity, the result in Figures 15 and 16 are not unexpected. The system is stable in 
velocity since the error covariance for both doppler u and v settle quickly to a steady 
state value before and after simulated submergence. The error covariance plots for 
heading and heading rate are shown in Figures 17 and 18 respectively. The numerous 


spikes in Figures 15 through 18 are from erroneous data from that respective sensor. 
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Figure 15: 6 State Error Covariance for Doppler u 
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Figure 16: 6 State Error Covariance for Doppler v 
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Figure 17: 6 State Error Covariance for Heading 
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6 State Heading Rate Error Covariance 
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Figure 18: 6 State Error Covariance for Heading Rate 
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The error covariance for heading and heading rate both settle to a steady state value 
very quickly. It should be noted that in the actual program for the six state filter the 
error covariances for all six state variables were set to the same initial value. In order 
to further reduce the innovation error and have the vehicle track closer to the actual 
path, sensor biases must be incorporated into the state vector. This allows the filter to 
adjust the state variables that are not getting updated by the associated bias state which 
is updated every time step. In the next section, the nine state filter results from the 
system model and measurement model developed in Chapter III section 5 are discussed 


and compared to the six state filter. 


C. NINE STATE FILTER 

The nine state filter incorporates bias states for both directions of velocity and the 
heading. In the analysis of this filter, those bias states will be plotted against both time 
and longitude to demonstrate where the filter bias states have reached a constant level. 
The data plotted in Figure 3 is the same data that is analyzed in this filter. Repeated 
the procedure for the six state filter in optimizing the R multiplier, Figure 19 shows the 


optimum value to be 1. Optimum in this case means the minimum of the average radial 
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- errot fora sét of R valies. ~~" 
1. Vehicle Tracking 
Figure 20 shows the comparison of the DGPS track, dead reckoning solution, and 
the nine state filter solution. In comparison with Figure 5 with the six state filter, it 


would appear that the nine state filter is not as accurate at predicted the tracked vehicle 
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vehicle path. As will be shown this is not the case when looking at the criteria of 
minimum radial error. Looking at the same range as Figures 6 and 7, Figures 21 and 
22 show the smoothing of the nine state filter. Comparing the Figures for the nine 
State to that of the six state, there is not much increased smoothing in the nine state as 
compared to the six state. Although this comparison is only looking at the position 
tracking where the biases were not directly applied. 

2. Heading Response 

The heading without the added bias compared to the measured heading, Figures 
23 and 24, exactly as it did for the six state mn Figures 8 and 9. 

3. Bias Effect 

Adding the bias to the doppler velocity did not change the comparison between 
the filtered output and the measured values as shown in Figure 25. Comparing this to 
the six state result from Figure 10 shows no improvement with the added bias. Since 
the measured doppler u is always available, there is no evidence of filter smoothing. 
This is not the case when looking at the heading with its bias. Due to the low variance 
of the heading data, a noticeable improvement was obtained as can be seen from Figure 
27, a close up of Figure 26. Comparing this with Figure 24, the heading without a 
bias, shows a marked improvement in the filters ability to track the actual data. With 
this improved filtered solution for heading and velocity, an improvement in position 


tracking would be expected. 


46 





150 


dr err (m) 


*oee#e © sete # ¢ # @ 


ee@e4as85ot e@#eqgqe eee 


ny 
es 
e 
° 
° 
* 
« 


eee ewe 


Aa 


sewers ee @weseeeneteteesetreesenetz@®reeere#neeetetree#e¢eeeerr#t? ¢ # 4 


s¢ee@«#e#st?e¢#s#8 @ 


soe wp tt & #88 et Fe O86 Oo 4 


ee eee? esos? 
eeteevpeenvneeteseeeseteeeoe tee ee ve Foe Qe ae 


o¢@ @ @ Bo a 


oes 


asesesee+get#s+8+rtreaeteeeteeeeteegse@t###eteetetese#8#etke* # ¢ 6 


' 
ao@espeeaeet8f @8 #€eeages 
oe@eseaetenes 


eee ev ew wm @ 


eesek8 #F 


s 
«ae 


eesmpeeeéee#t# tee 
> ¢@¢@e#ef & 


oepeege#eses 


se ee oveeo ee ome Qeeeeoee uve 


eee#enetrtee#se#st @ 


oeee eo qe eee 


ore#e 


9 State Average Radial Error 


ees 
ss e@e #@ @ 


aeaee@#?¢#t#trke¢##e## 


@eeeeatbtrbmetset*t#eeeetssaes 
Sa 


** te @e@ @ @ @ @ 


* 


Kaiman Filter 


- # © wae a 


Dead:Reckoning 


. 
= te &@ SeFotomes te FP avprergereay 


. 
seen se @es8 @ 
esaeeeeees 


Sr ee ee 
° 


«*¢eee8t#e88 #@ #&#& © & @ 
esa va 


. 
eseeeaeneewese ae 


eeetb+n@#eeseegee?#t##e#ts?t# @ 
' 


seespees@e@eeeen 
. 


eeegs 
see#s¢#ee8 # 8 





sea@#@ ep @t @@ #@ ho 4 @ 


> @e*eeese#8e?#*8 
*o@eteeeetkeesete# 
@eeee«etee##### serteeaesteesgte¢ 
see epee#e¢? Ce ee 
oe @ @ 


erteee«eserseteeeerte#e¢ 
. 


s 
eoese#eee#aeste##e#® 


eee @ eae 
se+e@#etee«#ter#teeetesea 
em 68 @ 


e? 


oJ 
*e*eee#e4@eetk##é#t*#¢#ees 


eeeneteesevte?t 


sees 


. 


eaeneensvsFeeereeeeeewnreaeaeeseee se 


oes?e#s#*n#ete 


eoene@#*?ee@#te#e# @##n#e#ee# ete @ & 


*ees 


see enmeeeeeaeaenveaet_eeeerses*aaes 


oh? @ me eee eo 


erecta: 


* 


eesvse#8# ##8 84 86 


eea#rtetee 


eee @@#@ ee ee @ @ 


ae @ 8 


@o@¢@¢@e@np @@@eeoe*e meee oe Pm eo @ 


sew @ @@ @ @ @ 


. 
eet sp eoee#s##? 
*6e eet # @ 


se#esrtseeeseseese#etese#s ss 8 
o¢#te8ee#*#@ 
oe @€e @@ 82 + © *® &@ @ @ @ @ 


eets 
eeeneaneneanandea 
s « 


*#enp es @ 6 8 


oaewteaevethanee 


oeee#ee8¢#¢ # @ @ 


eesersk## #6 @¢ 


> e@¢@e#eoereeere 


oe@e 


@eeee#treea 


see op @ @ @ @ see veeseeseseawa ere Pe ees 


eeesee#eea 


seeaeeans 


eeet?et?t 6s @ @ # @ 


Figure 19: 9 State Radial Error for Increasing R Multiplier 
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9 State Filter Path with Dead Reckoning and DGPS 
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Figure 20: DGPS Track. 9 State Kalman Filter, and Dead Reckoning 
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9 State Filter Path with Dead Reckoning and DGPS 
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Figure 21: 9 State Smoothing 
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Figure 22: 9 State Smoothing 
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9 State Filtered Heading and Measured Heading 
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Figure 23: 9 State Filtered Heading and Measured Heading 
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Figure 24: 9 State Filtered Heading and Measured Heading 
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9 State Doppler_u with Bias and Measured Doppler u 
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Figure 25: 9 State Doppler u with Velocity Bias 
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9 State Filtered Heading with Bias and Measured Heading 
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Figure 26: 9 State Filtered Heading with Bias and Measured Heading 
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Figure 27: 9 State Filtered Heading with Bias and Measured Heading 
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4. Innovation Error 

Figure 28 shows the innovation error for longitude and latitude for the nine 
State filter. Comparing this Figure to the corresponding output for the six state, Figure 
11, the nine state range of errors for longitude appear to be shifted to the left to those 
of the six state filter. This explains the nine state track being more to the left of the 
DGPS track in Figure 20 then the six state track in Figure 5. 

Figures 29 and 30 show the latitude and longitude innovation error separately. 

The nine state filter results in Figure 29 do not change as much as those in Figure 12 
for the six state filter in the area of vehicle submergence. Looking closely at these two 
Figures also shows that the nine state has less variance in latitude then the six state. 
Comparing Figure 30 to Figure 13 in the area of vehicle submergence, the nine state 
does not vary as much as the six state. There are other parts of the longitude 
innovation Figures that show the nine state to be more accurate then the six state. 

5. Radial Error 

The Figure showing the true performance of the filter, the radial error, for the 
nine state is shown in Figure 31. For all points in the mission, the nine state maintains 


a radial error less then that of the dead reckoning solution. 
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Figure 28: 9 State Innovation Error for longitude and Latitude 
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Figure 29: 9 State Innovation Error for Latitude 
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9 State Longitude Innovation Error 
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Figure 30: 9 State Innovation Error for Longitude 
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9 State Radial Error 
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Figure 31: 9 State Radial Error 
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Comparing the nine state radial error, Figure 31, to the six state radial error, Figure 
14, it can be seen that the nine state maintains the minimum radial error better then the 
six state when the vehicle submerges. Also the nine state varies much less then the six 
state for the remainder of the mission. Some of the reasons for the maccuracy of the 
six state filter are still present in the nine state. 

6. Error Covariance 

The assumption of zero cross correlation on the error covariance matrix is 
shown again to be invalid. — 32 shows that, as in the case of the six state filter, 
the doapiet u error covariance is cross soreiiied to the position due to the change 
when the vehicle submerges. This is also the case for doppler v in Figure 33 showing 
that there is a definite relationship between doppler velocity and DGPS updating as 
mentioned in the previous section. As in the case of the six state filter, the assumption 
for zero cross correlation for heading and heading rate variables is upheld in the nine 
state filter in Figures 34 and 35. Comparing the heading error covariance in Figure 34 
to that of the six state in Figure 18 shows more of a transition time for the nine state 
then the six state and the steady state values for the nine state are slightly higher then 
the six state. The explanation for the slight difference come from equation 2.9 in the 
propagation of the error covariance matrix. Since the A matrix is changed for er 
time step, the ¥ matrix is updated causing the update in the P matrix. This 
propagation for the error covariance matrix is not exactly the same for the nine and six 


state filters since the A matrices are different. 
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9 State Doppler_u Error Covariance 
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Figure 32: 9 State Error Covariance for Doppler u 
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9 State Doppler_v Error Covariance 
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Figure 33: 9 State Error Covariance for Doppler v 
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Figure 34: 9 State Error Covariance for Heading 
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9 State Heading Rate Error Covariance 
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Figure 35: 9 State Error Covariance for Heading Rate 
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There is a slight difference between the error covariance in the heading rate of Figure 
35 and the six state error covariance for heading rate in Figure 18. The main 
difference between the nine state and the six state lies in the usage of state variable for 
bias states for velocity and heading. The error covariance for the bias states should 
also drive toward a steady state value before submergence. 

7. Bias Learning 

As mentioned in the development of the nine state Kalman filter, the bias states 
were assumed to reach a steady State value, ‘learned’ before the vehicle submerged. 
Showing the heading bias for a complete surface run, Figure 36, clearly illustrates the 
constant bias assumption was incorrect. From Figure 36, the heading bias does not 
settle to some value until about 1400 seconds into the mission. Also note that this 
steady state bias is not zero as assumed in the initial state vector in the program 
ekf fau_9.m. Plotting the heading bias against filtered heading in Figure 37 shows 
that the heading bias is not constant with respect to vehicle heading either. Even based 
upon position, the heading bias varies throughout the mission as shown in Figure 38. 
These variations of heading bias all contribute to hindering the accuracy of the nine 
State filter. The variations of heading bias versus heading in Figure 37 and versus 
position in Figure 38 are to be expected. The variation is due to the presence of the 
earths magnetic field and the vehicles’ compass ineraction with that constant field. 
There is a effect on the heading providing by the magnetic compass from the internal 
iron. The bias for one given heading will not be the same for a different heading. 


Figure 39 shows that the doppler velocity u bias varies slightly over time and does not 
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reach a steady state until almost 1800 seconds into the mission. As with the heading, 
the doppler velocity u bias also varies slightly with respect to position as shown in 
Figure 40. The reason for this variation with respect to position can possibly be 
attributed to physical layout of the ocean bottom. Since the doppler sonar is bottom 
tracking, the bottom surface is not a even level plane and thus will cause variance in 
the bias to that sensor. Figures 41 and 42 show that same result for doppler velocity v 
bias as for the doppler velocity u bias, varying with respect to both time and position. 
The same results would be expected for both pee of the doppler velocity bias 
ote the same sensor measures all three components of the velocity relative to the 
ground and expressed in body coordinates. With the continuous updating of the 
heading and velocities, the error covariance for these variables would be expected to be 
driven toward a steady state value before submergence. 

8. Error Covariance Improvement 

Figure 43 shows the error covariance for heading bias reaching a steady state 
value before vehicle submergence as expected. The same holds true for the doppler u 
bias and doppler v bias as shown in Figures 44 and 45. Knowing that the biases for 
heading and velocity do not reach a steady state value before submergence based on 
: Figures 36, 39, and 41 an attempt was made and setting the initial state vector values to. 
the steady state values found from these results and setting the initial error covariance 


elements to the corresponding steady state values determined in Figures 43, 44, and 45. 
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Figure 36: 9 State Heading Bias 
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9 State Heading Bias and Filtered Heading 
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Figure 37: 9 State Heading Bias and Filtered Heading 
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Figure 38: 9 State Heading Bias and Longitude 
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Figure 39: 9 State Velocity u Bias 
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Figure 40: 9 State Velocity u Bias with Longitude 
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Figure 41: 9 State Velocity v Bias 
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9 State Velocity v bias with Longidute 
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Figure 42: 9 State Velocity v Bias with Longitude 
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9 State Heading Bias Error Covariance 
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Figure 43: 9 State Error Covariance for Heading Bias 
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Figure 44: 9 State Error Covariance for Doppler u Bias 


76 





9 State Doppler_v Bias Error Covariance 
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Figure 45: 9 State Error Covariance for Doppler v Bias 
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The purpose of resetting the inital values to the steady state values determined from the 
earlier run is to remove any initial offset the bias had from zero. In the first run of the 
Kalman filter, the bias states were initialized to zero instead of some off-set. Executing 
another surface run with these modifications did not greatly improve the initial results 
of a time varying biases. Figure 46 shows the heading bias after the modifications with 
no improvement over Figure 36 in reducing the variance of the heading bias over time. 
Plotting the new heading bias against filtered heading results in Figure 47. This is 
different then Figure 37 but no improvement then before the modifications due to 
explanation previously given. There was some slight improvement in the variance of 
heading bias against position in Figure 48 when compared to the before case of Figure 
38. In Figure 48 the heading varies between -1.6 degrees to about -0.75 degrees for 
longitudes between -300m to about -100m as compared to the before case where the 
heading bias varies between -1.5 degrees and 0 degrees for the same range in 
longitude. As for the doppler velocity u bias there was noticeable improvement in 
Figure 49 over Figure 39. The doppler velocity bias in Figure 49 settles to a steady 
State value of about -0.01m/s in a shorter time then shown in Figure 39. Also the 
amount of variance is less after the initial values are used then before. Against 
position, the doppler velocity u bias, Figure 50, still varies greatly with no 
improvement over Figure 40. As for the doppler velocity v bias not as much 
improvement was made as with the u direction. Figure 51 shows the doppler v bias 
settling much closer to the initial value of -0.01 m/s the before case shown in Figure 41 
which settles about -0.025 instead of the initial value of zero. 
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Figure 46: 9 State Heading Bias 
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Figure 47: 9 State Heading Bias and Filtered Heading 
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9 State Heading Bias and Longitude 
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Figure 48: 9 State Heading Bias and Longitude 
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Figure 49: 9 State Velocity u Bias 
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Figure 50: 9 State Velocity u Bias with Longitude 
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9 State Velocity v bias 
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Figure 51: 9 State Velocity v Bias 
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Against position, Figure 52 shows a very slight improvement in the variance with 
respect to position as compared to Figure 42. With the biases for the nine state 
analyzed, the comparison can now be done for the case if currents were added to the 
state vector instead of velocity biases. In the next section the results of the ten state 


filter will be presented and compared to the nine and six state cases. 


D. TEN STATE FILTER 

The ten state Kalman Filter enna two states for current and a aeacie rate 
bias instead of as nine state filters’ two velocity biases. The ten state filter is based 
upon an estimate for the current and more reliability on the model with current added 
to the relative velocities. An increase in the heading tracking ability is accomplished 
with the addition of the heading rate bias added to the heading rate. These 
developments are shown in the ten state model and measurement model of equations 
2.6 and 2.7 respectively. The noise weight for the heading bias is kept the same as 
for the nine and six state model for comparison purposes. Heading rate noise weight 
was determined in the same manner as for the nine and six state filters. The R 
multiplier was determined from Figure 53 and in the program the value of 10 was 


used. The R elements corresponding to the doppler velocities, heading, and heading 


rate were set to the same values as in the nine and six state filter. 
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Figure 52: 10 State Radial Error for Increasing R Multiplier 
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10 State Average Radial Error 










Figure 53: Average Radial Error vs R Multiplier 
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The elements corresponding to the relative velocities were adjusted so the filter 
would be more reactive to these velocities then the doppler velocities since the relative 
velocities are used as state variables. 

1. Vehicle Tracking 

The comparison between the ten state filter, dead reckoning, and the actual 
DGPS track is shown in Figure 54. From this Figure it can be seen that the ten state is 
only slightly more accurate then the dead reckoning solution. When comparing the ten 
State against the nine, Figure 19, or six state, Figure 5, the ten state is also less 
accurate then either one of these. Taking a closer look at a section of Figure 54, 
Figure 55, shows an advantage to the ten state filter. From Figure 55, the ten state 
solution tracks the vehicle more smoothly then either the nine state, Figure 21, or the 
six State, Figure 6. When DGPS is lost, during submergence, Figure 56 shows the ten 
state to be slower around the curve near the latitude of 500m then the nine, Figure 22, 
or the six, Figure 7. Thus even thought the ten state filter is very smooth, it can be 
Said that it is too smooth and is slow in approximating DGPS. This could be 
attributed to having the incorrect speed in the filter updating position from adding 
filtered currents to relative speed. The heading result, without the bias, is the same as 
that presented in analysis of the nine state filter. 

2. Heading Response 

The heading information in Figures 57 and 58 appear as the same as in Figures 
23 and 24 from the nine state, and the same as Figures 8 and 9 for the six state. One 


of the major differences between the ten state and the six or nine is the addition of 
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current variables added to the relative velocity. Figure 59 compares the filtered 
solution of relative velocity to the measured data. As can be seen for the same general 
time range of data as that of Figure 25, the relative velocity, measured and the filtered 
solution does not vary as much as the doppler velocity. Also from Figure 59, the 
filtered relative velocity does not track very reactively to the data, but maintains a more 
constant mean value. Looking into the accuracy of adding the currents to the filtered 
relative velocity results in Figure 60. 

3. Relative Velocity Response | 

As sya a the filter solution with the added current does not track the 
measured doppler velocity very well. This indicates that the current could not be 
correctly predicted by the filter. In comparison with Figure 25, adding the current to 
the filtered relative velocity does not approximate the true measured doppler velocity 
very well. This is probably the primary cause of the inaccuracy in the ten state filter in 
Figure 54. 

4. Bias Effect 

Adding the heading bias to the heading resulted in the same output, Figures 61 


and 62, as achieved in the nine state in Figures 26 and 27. Thus showing that no 


accuracy in predicting “heading was lost in adding the heading rate bias. 


5. Innovation Error 

As to the overall accuracy of position predicting, Figure 63 shows the large 
innovation error of longitude and latitude. These errors are more distributed and less 
concentrated in any one area of longitude or latitude as in Figure 28 for the nine state. 
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Figure 54: DGPS Track, 10 State Kalman Filter, and Dead Reckoning 
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Figure 55: 10 State Smoothing 
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Figure 56: 10 State Smoothing 
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Figure 57: 10 State Filtered Heading and Measured Heading 
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Figure 58: 10 State Filtered Heading and Measured Heading 
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Figure 59: 10 State Relative Velocity u and Measured Relative Velocity u 
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Figure 60: 10 State Relative Velocity u with Current u and Measured Velocity u 
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Figure 61: 10 State Filtered Heading with Bias and Measured Heading 
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Figure 62: 10 State Filtered Heading with Bias and Measured Heading 
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Figure 63: 10 State Innovation Error in Longitude and Latitude 
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Plotting the latitude and longitude innovation errors separately result in Figures 64 and 
65. Comparing these results to the nine state, Figures 29 and 30, and to the six state, 
Figures 12 and 13, the ten state filter never predicts the true DGPS track more 
accurately then either the nine or the six state filters. This result leads to the belief that 
the large errors in predicting current or more damaging to the filters' positional 
accuracy then the heading information. Looking at the comparison of the radial error 
of the ten state to the dead reckoning solution in Figure 66. The results are as expected 
the first comparison plot in Figure 54, the ten state is only slightly, at best, then the 
dead reckoning solution. comparing the ten state result Figure 66 to sheaine State 
Figure 31 and the six state, Figure 14, shows the ten state performs far worse then the 
nine or six State filters. 

6. Error Covariance 

To further illustrate the source of the ten state filters’ inaccuracy, the error 
covariance for relative velocity is presented in Figure 67. The variance is very large 
and no cross-correlation is evident, as 1n the nine state, Figure 32, and the six state, 
Figure 15. As shown in the relative velocity plot of Figure 59 and with the added 
current in Figure 60, the filtered velocity does not track closely to the measured 
results. The large jumps in Figure 67 are where a measurement is received and the 
error covariance 1s updated. As can be seen from Figure 59, the update does not track 
the prediction to the actual data due to the choices in the R matrix. As expected, the 
same result for the relative u velocity is obtained for the relative v velocity as shown in 
Figure 68. 
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Figure 64: 10 State Innovation Error for Latitude 
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Figure 65: 10 State Innovation Error for Longitude 
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10 State Radial Error 
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Figure 66: 10 State Radial Error 
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10 State Relative_u Error Covariance 
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Figure 67: 10 State Error Covariance for Relative u 
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The error covariance for the heading is shown in Figure 69, which illustrates some 
difference then the nine state of Figure 34. The settling time for the ten state is slightly 
longer then the nine state and the steady state value is slightly lower. Since there is a 
definite relationship between a change in position and heading, the difference in the ten 
states' position accuracy and the nine states’ could explain the difference. What is 
important is that the ten state heading bias does reach a steady state value. The error 
covariance for the heading rate in Figure 70 very similar to that for the nine state, 
Figure 35. The noticeable difference is the longer transition time for the ten state then 
the nine state. The two addition state variables in the ten state filter, current X and 
current Y in Figures 71 and 72, respond similarity to the relative velocities. The 
jumps in this Figure are due to the same reason as that for the relative velocities, an 
update of the relative velocity measurement. The actual current added to the relative 
velocities is very small when compared to the relative velocities. Thus, the error 
covariance will respond nearly the same as for the relative velocities. The heading bias 
response for a surface run is shown in Figure 73. The heading bias is not constant as 
assumed in the problem development and reaches a steady state value quite different 
from the nine state, Figure 36. Also apparent is the decrease in roughness present in 
the nine state filter: This is to be expected since from the vehicle path plot in Figure 
55 shows a smoother, and slower, filter. Due to the improved smoothness of the ten 
state filter, not only is the heading bias smoother then the nine state, but also the 
heading variance with respect to filtered heading, Figure 74, is also less then that 
shown for the nine state in Figure 37. 
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Figure 68: 10 State Error Covariance for Relative v 
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| Figure 69: 10 State Error Covariance for Heading 
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Figure 70: 10 State Error Covariance for Heading Rate 
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Figure 71: 10 State Error Covariance for Current X 
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Figure 72: 10 State Error Covariance for Current Y 
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Figure 73: 10 State Heading Bias 
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Figure 74: 10 State Heading Bias and Filtered Heading 
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Figure 75: 10 State Heading Bias and Longitude 
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The ten state heading bias with respect to position also varies much less then the nine 
state heading bias as shown in Figure 75 for the ten state and Figure 38 for the nine 
state. As for the heading bias error covariance, the results for the ten state and nine 
state are similar in the settling times. Comparing Figure 76 for the ten state to Figure | 
43 for the nine state shows the ten state reaches a different steady state value in about 
the same amount of time. From the earlier plots for the heading bias, this is to be 
expected since the ten state steady state heading bias value was different then the nine 
state. The additional ten state variable of heading rate bias was shown from Figure 62 
not to be significant since the filtered heading with bias was very close to the measured 
heading and to the results obtained from the nine state filter, Figure 27. Figure 77 
shows that the ten state heading rate bias reaches a steady state value of near zero fairly 
quickly. Looking at the results for the error covariance for heading rate bias, Figure 
78, the error goes to zero by the time submergence is reached. Thus, the extra state 
variable of heading rate bias did not make an improvement in the accuracy of 
predicting heading or position. 

7. Error Covariance Improvment 

The procedure for the nine state filter were modifications were done to get the 
‘bias ‘states to reach a steady state value before submergence were repeated for the’ ten | 
State filter. Figure 79 shows that no improvement was made to the heading bias 
reaching steady state any sooner then Figure 73. Also there is no reduction in the 
variance of the heading bias. Thus no improvement is found in Figure 79 for heading 
bias against filtered heading. 
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Figure 76: 10 State Error Covariance for Heading Bias 
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Figure 77: 10 State Heading Rate Bias 
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Figure 78: 10 State Error Covariance for Heading Rate Bias 
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Figure 79: 10 State Heading Bias 
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There was also no reduction in heading bias variance with respect to position as shown 
from comparing Figure 81 to Figure 75. The only noticeable improvement in filter 

accuracy was with the heading rate bias. In Figure 82, there is a definite reduction in 
the variance of the bias then before the modifications in Figure 77. With the results for 
the six, nine, and ten state filters presented and analyzed, a qualitative comparison and 


general conclusions among the three filters will be discussed in the following Chapter. 
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Figure 80: 10 State Heading Bias and Filtered Heading 
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Figure 81: 10 State Heading Bias with Longitude 
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Figure 82: 10 State Heading Rate Bias 
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V. CONCLUSIONS 

A. AYSNCHRONOUS FILTERING 

This thesis has developed several Kalman filters to aid in solving the navigation 
problem for small low cost autonomous underwater vehicles. The major problem 
addressed was the fussion of incoming data from sensors arriving asynchronously 
between the sensors and between time steps of the same sensor, as shown in Figure 2. 
For simulation purposes and post-processing the data from Florida Atlantic University, 
the mission data file had to be pre-processed to ensure that information was present at 
every rae step, being repeated when no new information was available. The as 
order hold effect developed in Chapter II section B worked quite well with the smooth 
response of all three filters shown in Chapter IV. For all measurements analyzed in the 
six, nine, and ten state filters, there were instances where information was not available 
from the sensor. All responses for all of the state variables in all three filters 
responded smoothly and completely with no loss of track for any of the state variables. 
The effect of adding biases to the heading and velocities of the nine state filter had little 
effect on smoothness response over the six state as shown in the results, Chapter IV 
section A. However, with the currents added to the relative velocities in the ten state 
filter, there was a large improvement in the smoothness of the predicted track. Even 
with greatly scattered measurement data, as with the velocity measurements, all three 
filters smoothly predicted a "averaged" velocity. In the analysis of the heading 
response, where the measurements are accurate and very little variance, where there 
was no data, all three filters smoothly predicted the heading and tracked closely to the 
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future incoming data. The problem of aysnchronous data processing being solved, the 
next major problem was accurately predicting the actual vehicle path. Three filters 
were analyzed against the basic dead reckoning solution and different methods of 
increasing the basic six state system model. The next section will discuss the 


comparison and advantages and disadvantages of each. 


B. STIX, NINE, AND TEN STATE FILTER PERFORMANCE 

The six state did predict the tracked path more accurately then the dead reckoning 
solution. State a for heading, heading rate, doppler u , and doppler v tracked 
closely to the measured data. The main advantage of incorporating errors with the 
Kalman filter comes clear in the solution of predicting the vehicles’ position. As the 
development of the six state Kalman filter predicted, the error covariances for the state 
variables which measurements were available settled to a steady state value before 
simulated submergence. This implies that a constant gain also results before vehicle 
submergence. It was also shown that the assumption in the development of the 
Kalman filter that the cross correlation is zero between all state variables for all three 
filters. This was shown for all three filters to not be the case. There is definite cross 
correlation between the doppler velocities and position from the change in the error 
covariance from one steady state value to another after the vehicle submerges. For the 
nine and ten state filters the assumption was also made that the bias states were 
constant and could be "learned" before vehicle submergence. This was shown not to 
be case for all of the bias states. The only exception to this was the heading rate bias 
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which did reach a steady state value of zero toward the end of the mission. All of the 
bias states were also assumed to start with an initial value of zero in the program, this 
was also proven to be incorrect. The attempt at bringing the bias states to a steady 
state value before submergence with initial bias states set to values from a surface run 
and setting the elements in the covariance matrix to steady state vales did improve the 
variance of the bias states. The variation of the heading bias was shown to vary with 
respect to filtered heading and position for both the nine and ten state filters. The 
assumption used with the six and nine state filters of no current was shown to be false 
as expected ron the oe of the sHiperon for longitude and latitude. Once he 
current states were included in the ten state filter, the prediction was not correct. 
When the current was added to the relative velocity there was still a large error 
between this result and the measured doppler velocity. This method of computing the 
doppler velocity caused the severe reduction in accuracy of the position predicting 
ability of the ten state filter. 

From the above comparison and the results presented in Chapter IV, the filter to 
best predict the vehicles path in the presence of an actual current is the nine state filter. 
This filter had the lowest radial error for the entire mission and the advantages of 
smoothness over the six state filter. This filter showed that the bias states were needed 
on the velocity and heading measurements from the improved accuracy over the six 


state filter. 
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C. RECOMMENDATIONS 

The MATLAB program developed in this thesis with the mathematical model in 
the Kalman filter need to be fine tuned for better performance, especially for the nine 
State filter. Elements in the R matrix for the nine and ten state filter need to be further 
adjusted to achieve a steady state value for the bias states prior to submergence. The 
code for all three filters must be converted to a real time code, 'C’, for true time 
response testing. The data used in this thesis was from a purely surface run and thus 
did not reflect true changes in velocity and other vehicle attitudes when actually 
submerged. Data from Florida Atlantic University from a completed 
surface/submerged run needs to be analyzed for more thourgh evaluation of compass 
bias. As mentioned in the first section of this Chapter, constant gains were developed 
as a result of the error covariance elements going to zero for all states except position. 
This implies that after the successful conversion of the current MATLAB code to C 
it could be tested on the Ocean Voyager II. It has been proposed that additional states 
be added to the ten state filter so that more complex bias models could be analyzed for 


imroved accuracy when submerrged. 
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APPENDIX A. KALMAN FILTER PROGRAMS 


Mar 15 1997 18:04 ekf_fau_6.m Page 1 


clear 

% SSET UP TEST MOTION OF VEHICLE AND MEAUREMENT VECTOR 
% 

load dgps_0827_1152_rel; 

d=dgps_0827_1152_rel; 
gpsSec=d(:.1);gpsStatus=d(:,2);hdop=d(:,3);pitch=d(:.4); 
roll=d(:,5);heading=d(:,.6) ;yaw_rate=d(:,7);ug=d(:,8); 


vg=d(:.9);wg=d{:.10);long=d(:,11);lated(:,12); 


startSample=S00; 
endSample=16313; 


long=long*3600* .51*0.89; li=long(star¢tSample) ; 
lat=lat*3600*.S1;1l2=lat(startSample) ; 
long=11.*ones(length(lat),1)~-1long; 
lat=lat-l12*ones(length(lat),1); 

dt=8/60; 


t=0:dt: (length(ug)}-1)*dt; ttime vector 
tcheck for bearing ambugity between 360 and 000 degrees 


for izstartSample:endSample-1; 


if heading(i)> 180.0, heading (i) heading (i)~-360.0;end; 


end; - a 
n=O0;mpsisheading; 
for izstartSample:endSample-1; 


i£ abs(heading(i+1)-heading(i))> 180, n=n-1*sign (heading (i+1)-heading(i));en 
a; 
mpsi(i+1)sheading(i+1}+360.0°n; 
end; 


heading=mpsi; 


mult={.1 1 10 100 1000 10000]; 
for N=1:6; 


&MEASUREMENT VECTOR (8) 
y=( ug. vg, heading. *pi/180, yaw_rate.*pi/i80, lat, long, ]; tcomplete measured data 


% STATE VECTOR 

% x{:,.s)#{lat(s),long(s),psi0, yaw_rate(s) *pi/180,dop_ug, dop_vg]’: 
: 4 

psi0=heading (startSample} *pi/180; 


Giniialize the state vector to measurements values 

x=zeros (6,endSample) ;err=zeros(6,endSample) ; 

s=startSample; 

x(:,s)=flat(s),long(s),psi0, yaw_rate(s) *pi/180,ug(s).vg(s)}"; 


tMANEUVERING AND CURRENT TIME CONSTANTS 


taul=1; 
tauz=1; 
tInitial A matrix 


fl=ug*cos(psi)-vg*sin(psi); 
f2=ug*sin(psi)+vg*cos (psi); 
f£3=r; 
£4=0; trdot=0; 

.. £5=0; tugdot=0; : 
£6=0;t%vgdot=0; 


dP oP dP dP dP oF 


A=zeros (6,6); 

X=x(1,s);Y=x(2,s) ;psi=x(3.s);r=x(4.s); dop_ug=x(5,s) ;dop_vg=x(6,8); 
A(l, 3)=-dop_ug*sin(psi) -dop_vg*cos (psi); 

A(1,5)=cos(psi); 

A(1,6)=-sin(psi); 

A(2.3)=dop_ug*cos (psi) -dop_vg*sin(psi) ; 


Program 1. MATLAB Code "ekf fau_6.m" 
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Mar 15 1997 18:04 ekf_fau_6.m Page 2 | 


A(2,S)esin(psi): 
A(2,6)=cos(psi); 
A(3,4) 41; 


% y=(ug.,vg, compass, yawrate,gpsXx, gpsY) 
RInitial ¢C matrix 

Czzeros(6.6); 

C(1.S$)21: 

C(2.6)=1; 

C(3,3)21: 

C(4.4) 21; 


C(S.1)21: 
C(6,2)21:; 


a 


D=zteros (3.6); 

@Initial B matrix 
qi=0.3;*variance on X, m*2 
q2=0.3;%variance on Y. m*2 
q3=0.01;ttvariance on psi. rad*2 
q4=0.01;% variance on fr, rad/s)“2 
q5=0.015;% variance on ug, (m/s) *2 
qg620.015;% variance on vg, (m/s)%2 


B= {ql:q2:q3:q4:q5:q6]; 


Q=diag(B); 
tsystem noise 


mul\l00; nu2=100; nu3=<0.1; nud#0.25; muS=1010; nu6=1010; 
gnus(nul;nu2;nu3;nué;nuS;nué) .*mult(N); 


R=ediag(gnu); & measurement noise 
psavezzeros(6,5500); 
eae old_after means measured data at old time, new_before means model predicted v 
p_old_afterzeye(6)*le-1; 
delx_old_after=zeros (6,1); 
gzones (6,1) ;psave=zeros(6,2500); 
for i=startSample: (endSample-1): 
tcompute linearized PHI matrix using updated A 
(phi. gam}=c2d(A,B,dt); 
Rcam=(eye(6,6)+A. °dt/2+A°2. edtrdt/6] .*dt; 
tphisexpm(Ardt); 
&reset initial state 
x_old_after=x(:,i): 
* nonlinear state propagation 
x_new_before=prop6 (x_old_after, taul, tau2,dt): 
terror covariance propagation 
p_new_before=phi*p_old_after*phi’ + Q: 


tnew gain calculation using linearized new C matrix and current state 
error covariances. 


Program 1. MATLAB Code "ekf_fau_6.m" (cont) 
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formulate the innovation using nonlinear output propagation 
tas compared to new sampled data from measurements 


yhat=outputé (x_new_before) ; 


err(:,itl)=(y(it+1,1:6)° - yhat); 


if y(it+l,1)=-y(i,1),C(1,:)=0.*g’ ;end; 
if y(it1,2)=sy(i,2),C(2,:)=0.*g' :end; 
if y(it+1,3)==y(i,3),C(3,:)=0.*g' ; end; 


if y(itl,4)==y(i.4),C(4, :)=0.*g’ ;end; 
if y(itl,5)=ey(i,5),C(S5,:)=0.%g' send; 
if y(i+1,6)==yv(i.6),C(6,:)=0.%g’ ;end; 
if i>=4001; C(5,:)=0.0%g':C(6,:)=0.0%g’ ;end;%eliminate dgps 


G=p_new_before*C’ *inv([C*p_new_before*C’+R]); 


cpc=C*p_new_before*C’ +R; 


. vrel(:,i¢l1)=err(:,it1).*2./diag(cepc); 


--$ -conpute ‘gain,: update toal state and error covariance 


p_old_after=[eye(6) - G*C]*p_new_before; 
psave(:,i+1)=diag(p_old_after) ; 
x_new_after=x_new_before + Grerr(:,i+1); 


tcarry new state into next update 
x(:,i+1)=x_new_after; 
resetting up the linearized A matrix 


A=zeros (6,6); 
X=x(1,i¢1) :¥Y=x(2,i+1) spsi=x(3,i+1);r=x(4,i+1) ;dop_ug=x(5,i+1}; 
dop_vg=x(6,i+1); 


A(1,3)=-dop_ug*sin (psi) -dop_vg*cos({psi) ; 
A(1,5)=cos(psi); 

A(1,6)=-sin(psi); 
A(2,3)=dop_ug*cos (psi) -dop_vg*sin (psi) ; 
A(2,5)=sin(psi) ; 

A(2,6)=cos (psi); 

A(3,4)=1; 


treset the linearized C matrix 
C=zeros (6,6); 

C(1,S5)=1; 

C(2,6)=1; 

C(3,3)=1; 

C(4,4)=1; 

C(5,1)=1; 

C(6,2)=1; 


end; 


%do deadreckoning solution 

Xa@r=zeros(1,length(t)) ;¥dr=Xdr; 

for i=startSample: (endSample-1), 
pp=heading (i}*pi/180; 
fl=ug(i)*cos(pp) -vg(i)*sin(pp); 
f2=ug(i)*sin(pp)+vg{i}*cos(pp); 
Xdr(i+1)=Xdr (i)+dt* (£1); 
Ydr (i+1)=Ydr(i)+dat*t2; 


Program 1. MATLAB Code "ekf_fau_6.m" (cont) 
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end: 


& compute radial error 

Xerr(startSample:endSample) =lat(startSample:endSample) -Xdr(startSample:endSample)’: 
Yerr (startSample:endSample)=long(startSample:endSample)-Ydr(startSample:endSample) ‘; 
raddr (N) «mean (abs (Xerr+¢j*Yerr) }; 

Sraddr-abs (Xerr+j*Yerr) ; 


tceompute radial errror 


Xfiltererr(startSample:endSample) slat (startSample:endSample) -x(1,startSample:endSamp 
le)’; 

Yfiltererr(startSample:endSample) =long(startSample:endSample)-x(2, startSample:endSam 
ple)’; 

radfilter(N)=mean(abs(Xfiltererr+j*yfiltererr)); 

tradfilter-abs (Xfiltererr+j*yfiltererr): 

end; 


figure(l} 

pilot (t(startSample:endSample) ,x(3,startSample:endSample) .*180/pi.’y:’,... 
t(startSample:endSample),heading(startSample:endSample),‘g+’), grid 
title(’6 state filtered heading, dots. compass, +’) 

ylabel(‘heading (degrees) ‘) 

xlabel(’time {sec)‘) 


figure(2)} 

plot (long(startSample:endSample), lat(startSample:endSample),’g--'), grid 
bold on 

plot(x(2,startSample:endSample),x(1,startSample:endSample), ‘c-.’) 

plot (Ydr(startSample:endSample) ,Xdr(startSample:endSample), ‘y’) 
title(’'6 State Filter with Dead Reckoning and DGPS’) 

hold off 

xlabel(’wW Longitude (m) E’) 

ylabel('S Latitude (m) WN’) 


figure(3) tdoppler u 
plot(t(startSample:endSample),ug(startSample:endSample),’g+’,... 
t(startSample:endSample),x(5, (startSample:endSample)), 'yx’),grid 
title(‘Doppler u, + and estimated u, x vs Time sec’) 


figure (4) 
piot(err(6, startSample:endSample),err(5,startSample:endSample),‘go’).grid 
title(’Y error vs X error ’)., zoom 


figure(s) 

subplot(2,1,1) 

pict(t(startSample:endSample), radfilter(startSample:endSample)),grid 
title(‘'6 state filter radial err’) 

grid 

xiabel(’time (sec)’) 

ylabel(’ filter err (m)’) 


Subolet (2.1.2) 

plot(t(startSample:endSample), raddr(startSample:endSample) ) 
grid 

xlabel(’time (sec)’) 

ylabel(’ DR err (m)’) 


wfilter performance 


tmaxerrs=(norm(rel(1,622:680),inf);... 

fmorm(rel(2.622:680),inf):... 4 
fnorm(rel(3,622:680),inf}:... 

tnorm(rel (4,622:680) ,inf);:... 

tnorm(rel(5,622:680),inf);:... 

tnorm(rel(6,622:680),inf);... 

fnorm(rel(7.622:680),inf);:... 


Program 1. MATLAB Code "ekf_fau_6.m" (cont) 
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snorm(rel(8,622:680), inf) ] 

%J = mean(rel(:,startSample:endSample)‘)', % size(J) = m*l. 
%bud=mean (psave(:,startSample:endSample)’)'; 

terr_bud=bud. /sum(bud) ; 


figure (1) 

semilogx (mult, radfilter, ’c’,mult, raddr, ‘y’) 
grid 

ylabel('err (m)’) 

xlabel(’R mult’) 

title(’Radial error vs increasing R’) 


Program 1. MATLAB Code "ekf_fau_6.m" (cont) 
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Mar 16 1997 14:01 ekf_fau_9.m Page 1 


clear 
3 SSET UP TEST MOTION OF VEHICLE AND MEAUREMENT VECTOR 


a P5_0827_1152_ re} 
3575_0827_1152 re): 


gpsSec=di:.1); gpsStatus=d(:,2): hdop=d(:.3): pitch=dt:,¢)- 
rell=d(:,5); heading=d(:,6): yaw_rate=a(:,7}; 


dcp_u=d(:,8); dop_v=d(:.9); dop_w=d(:,10}: Lengsat 11+; Latest (e. ist: 
u_rel=d(:,13):v_rel=\d(:.14);w_rel=d(:,15S)- 

StartSample=$9°6: 

endSample=163i13; 

long=long*366C*.51°0.89: ll=long(startSample): 

dat=late3s90° $1; l2=lat(startSampie}; 
long=1l.*ones(length(lat).1)-long; 

lat=lat-l2*cnes(length/lat),1): 


i{lengthidop_u)-1l)°dt; &time vector 
tcheck fer bearings ambugity between 360 and 000 desrees 


cece i=startSarple:endSample-1; 


if heading(i)> 180.0, heading(i)=heading(i)-369.0:end- 


i= abs (heading(i+1)-heading(i)}> 180, n=n-l*sign (heading (i+i)-heading(i}):en 


a: 
mpsi{i+l)sheading(i+1)+360.0¢n: 

end 

heading=rnsi 

molc=(.01 .1 1 16 100 1060 10000]; 


ps: 0-heading(startSample)*pi/iso: 
SMEASUREMENT VECTOR (8) 

yr{t’.dcp_u, dor _v, heading. *pi/iso, yaw_rate.*pi/1B80, lat. long?: complete measure 
c data 

% SYSTEM NOISE / STATE VECTOR 

% sieoe bal taste): tengts)psi0.doruls).dep vis) ,bulslube(s iyaw Besers|@oiyiEo ties 
}- 

% 

G+=°.3: tsert(variance on lat), m 

G2= 2.3: sqrt(variance on long}.m 

q@3*0.0l;:%&sqrtivariance on psi),rad 

qQs=°.O015;&sgrt(variance on dop_u),m/s 

GS=°.0iS;%sqrt(variance on dop_v),m/s 

GSO: %sqrt {variance on bu). m/s 

q@?7=C; &sgrtivariance on bv). m/s 

Gf=C0.01: tsqrtlvariance on r), rad/s 

Qo=0: &sqrt(variance on bsi). rad 

&inilalize the state vector 

x=zercs(9,endSample}: err=zeros(6,endSample); 

S=startSanxrple:; 

xor-s)=(lat(s},long(s},psi0, dop_u(s),dop_v(s).0,0,yaw_rate!s)*pi/160.61: 


RMANEUVERING AND CURREMT TIME CONSTANTS 


taulel: 


Program 2. MATLAB Code "ekf_ fau_9.m 
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_€(1,4)=1; C(1,6)=1; 
(C2, 5)21: C(2,7) =1; 
C(3,3)=1; C(3.9)=1; 





ekffau 9m. 





tau2=1; 
Initial A matrix 


A=zeros (9,9); 

X=x(1,s); Y=x(2,8); psi=x(3,s); dop_ug=x(4,s); dop_vg=x(5,s); bu=sx(6,s); 
bv=x(7,S); r=x(8,s); bsi=x(9,s); 
A(1,3)=-dop_ug*sin (psi) -dop_vg*cos (psi); 
A(1,4)=cos (psi); 

A(1,5)=-sin(psi); 
A(2,3)=dop_ug*cos (psi) -dop_vg*sin(psi); 
A(2,4)=sin(psi); 

A(2,5)=cos(psi); 

A(3,8)=1; 

A(4,4})=-1/taul*d; 

A(5,5)=-1/taul*o; 

A(6,6)=-1/tau2*0; 

A(7,7)=-1/tau2*0; 

$tIinitial B matrix 
B=[ql:q2;q3;4q4;q5;q6:q7;q8;:q9]; 

Initial ¢C matrix 


C=zeros(6,9}; 


C(4,8)=1; 
C(S,L)e<1; 
C(6,2)=1; 


D=zeros(6,9); 
nuil=100.00; mu2=100.00; nu3=.100; nu4=0.2500; nu5=1010.00; nu6=1010.0; 


gnu= [nul;nu2;nu3;nu4;nu5;nu6;).*mult (Nn); 
R=diag(gnu); % measurement noise 
Q=diag(B); tsystem noise 


Note, old_after means measured data at old time, new_before means model predicted v 
alue 


tp_old_aftersdiag([{.5 .5 0.2598 1.3481 1.4944 0.4299 0.4324 0.0514 0.2211]) ; 


piold_after=eye(9)*le-i; 


Gelx_old@_after=zeros(9,1); 


g=ones(9,1); psave=zeros(9,endSample)} ; 
for i=startSample: (endSample-1) ; 


tcompute linearized PHI matrix using updated A 
(phi, gam]=c2d(A,B,dt); 
gam=eye (10,10); 
tphizeye(10,10)+A*0.12S; 
¢reset initial state — 
x_old_after=x(:,i); 
% nonlinear state propagation 


$écest=phi*x_old_after; 
x_new_before=prop9 (x_old_after,taul,tau2,dt); 
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Rerror covariance tropagation 
r x 


p_new_before=phi*p_old_after*phi’ + Q;: 
m Calculation using linearized new C matrix and current state 
vay 


bicrmulate the inncvation using nonlinear output propagation 
tas compared to new sampled data from measurements 
yhat=output?(x_new_before): 


ers(:,dslie(y(iel, 2:7)" = yhatt: 


Be Yl ae lceiesy (soy Ct )=0.0°g’: end: 
f oyfiel. 3) e2y(i.3:.0(2,:)20.0%g’ send: 
if yliel, 6) =2961.6).C(3.:)20.0% send: 
if yliel. 5) ==9 (4.51.66, :)20.0%¢’ send 
if yliel,6)==y(i.6!.C(5.:)=0.0%g'; end 
if yliel. 7)ssyli.7).C(6.:)20.0%g’ send 


RSimulate going submerged and } 


Oo 


ss of DGPS updates 
RE i>=6002; C(5,:)29.0°g' 5 C(6,:)=0.0% S's end: 


cre*C’*invlC*p_ new_before*C’ + R): & Kalman Gain 
Sfie*l)-x_new_before({1)), (long(i+l)-x_new_before{1l))}: 


wState error covariance estimate update 
cro=c°p_new_before*t’ +R; 

relts.4+2. )2err (se. i¢l) 272. /dlaetepey: 

& rei(:,i¢l)sdiag(err(:,i¢l)*inviepe) *err(:,i41)'); 

% cconpute gain. update toal state and error covariance 
p_cidc_after=[eye(9} - G*C}*p new _before: 
psavel:,ie*l)sdiagip_old_after): 
x_new_after=x_new_before + G*terr(:,i¢1): 

tcarry ne« state inte next update 


x(:,i*¢l)=x_new_after: 
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aszeres!'9,9); 
Hexfi.de)): Yex(2,iel); psi=x(3,i41l): dep_ug=x{4,iel); dop_vg=x(5,3¢1): busx 6, 
Beex (7,241); rex(@.iel); bsiex(9,iel): 

Ati, jJ)2-dop_ug*sin(psi)-dop_vg*cos (psi); 
A'i,é)=cos(psi); 

A’... Si=-sin(psi);: 
A'z.3)=dcp_ug*cos(psi)-doep_vg*sintpsi): 
AlZ.4)=ssintpsi): 

A'2.S)=scos(psi); 

Al3,8)e21; 

Af4.$)=0; 

AtS,S)=0; 

AE,6)=0; 

A(7,7)=0; 


—.ekffau_9.m 


reset the linearized C matrix 
C=zeros({6,9); 

C(1,4)=1; €(1,6)=1; 

C(2; 5) 212: Cl2, i=l; 

C(3,3)2i: €(3.9) 21; 

C(4,8)=1; 

C(5,1)=1; 

C(6,2)21; 


end; 


est_lat(startSample)=lat(startSample) ; 
est_long(startSample)=long(startSample) ; 


for i=startSample: (endSample-1), 
est_lat (i+1)=(dop_u(i)*cos (heading(i).*pi/180)-dop_v(i)*sin(heading(i).*pi/180)}* 
dt+est_lat(i); 
est_long(i+1)=(dop_u({i}*sin(heading(i) .*pi/180)+dop_v(i) *cos (heading(i).*pi/180)) 
*dt+est_long(i); 


end; 


Xerr (startSample:endSample)=lat(startSample:endSample) -est_lat(startSample:endSample 
ae 

Yerr (startSample:endSample) =long(startSample:endSample) -est_long(startSample: endSamp 
le)’; 

raddr (N)=mean (abs (Xerr+j*Yerr)); 

raddr=abs (Xerr+j*Yerr) ; 


Xfiltererr (startSample:endSample)=lat(startSample:endSample)-x(1,startSample:endSamp 
le)’; 

Yfiltererr(startSample:endSample)=long(startSample:endSample) -x(2,startSample:endSam 
ple)‘; 

radfilter(N) =mean(abs(Xfiltererr+j*yfiltererr) ); 

tradfilterz=abs (Xfiltererr+j*Yfiltererr) ; 


end; 


figure(i) 
plot(t(startSample:endSample),x(3,startSample:endSample).*180/pi, ’yx’,.. 
t(startSample:endSample) ,heading(startSample:endSample),’g+‘),grid 
title(’'9 State Filtered Heading and Measured Heading’ ) 

xlabel(’Time (sec) ‘) 

ylabel (‘heading angle (degrees) ’) 

zoom 


figure (2) 
plot(long(startSample:endSample),lat{startSample:endSample),‘g--’,... 
x(2,startSample:endSample),x(1,startSample:endSample),‘c-.') 

hold on 
plot(est_long(startSample:endSample),est_lat(startSample:endSample) } 
xlabel(’W Longtitude (m) E’} 

ylabel(’S Latitude (m) WN’) 


title(’9 State Filter Path with Dead Reckoning and DGPS’) 
grid 

hold off 

zoom 


figure(3) tdoppler u 
plot(t(startSample:endSample),dop_u(startSample:endSample),‘gx’,... 
t(startSample:endSample),x(4,startSample:endSample)+x(6,startSample:endSample),... 
"e")!, Gri 

title(’9 State Doppler_u with Bias and Measured Doppler u’) 

xlabel(‘time (sec)’) 
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ylabel('’speed (m/’s}') 


e2oon™. 


ms KOT ry we 
0 4 bee rg Be bee 


analysis for t=(622:650) 

for heading (compass signal / measurement} 

tSample:endSample)+x(9,.startSample:endSamrle} ; 
mple:endSample); 
headhatz=heading‘startSample:endSample)‘; 
plet(tt.psihat.°1B0/pi., ‘yx’. tt. headhat,‘g+').grid 
title(’9 State Filtered Heading with Bias and Measured Headings’ | 
xlabell(‘time (sec)’) 
ylabel(‘heading (degrees)') 


Sandan nial 
Ges 


HF SO rH 
Ty" pe 


eT 
rh uy 4 


figere'é) 

Sea leet2 cls) 
picc(t . redttleces 
grid 


O tr 
{i re TS 


pee 


s 
F 
g 
x 
y 


- 
i 
~ 
- 
~ 
a 
~ 
1 


& G 


pict (hE. 409.54). *2 80. 
ylabel(’ heading bi 
xlabel(‘time (sec}' 


(+ fagotto 


ficsure (8) 

Pict (loeng 

ylabel(’ head 

xlabel(‘Longi 
LA 


(se7)') 


“ * QU ra 
+ Ro 4 Epa pee 


bias’) 


Velocity u bias with Longidute’) 


startSample:endSample).x(7,startSample:endSample!}) 
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ekf_fau_9.m 





grid 

xlabel(‘time (sec)') 
ylabel{’m/s’) 

title(’9 State Velocity v bias’) 


figure(12) 

plot (long(startSample:endSample),x(7,startSample:endSample) ) 
grid 

xlabel(‘Longitude (m)’) 

ylabel (‘m/s’) 

title('9 State Velocity v bias with Longidute’ ) 


figure(13) 

prot (x(3, startSample:endSample) .*180/pi,x(9, startSample:endSample) .*180/pi) 
grid 

xlabel('Filtered Heading (degrees) ‘) 

ylabel (‘Heading Bias (degrees) ') 

title(’9 State Heading Bias and Filtered Heading’) 


figure(1} 

semilogx (mult, radfilter,'c’,mult, raddr,‘:') 
grid 

ylabel(‘dr err (m)’) 

xlabel(‘'R mult’) 

title('9 State Radial error vs increasing R') 
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SSET UP TEST MOTION OF VEHICLE 


aeusd(s:. 292 hdss=e 0s, 3) prtensats.6)> 
heading=d(:.6): yaw _ratesd(:.7); 
(2. 9)2 dop wed (2,10); dongedt:, 21); latset:,.12) 
eer? Oe fe Mss ae “at Gaara > Bs I 


startSamcle=-S0¢; 
ample=si6313; 
ms°3009"..51°C-89> Jil=loneitstartsanc lel: 
*3690°¢ 51; l2slazistartSarole): 
.*onestlength(lat),1)-long:; 
«<2*cones(lerack (iat)..1)3 


:fliength(dop_uy<+)) "dt; &oime vector 


ambuqdity between 36¢ and 00C dearecs 


i=startSactple:endSample-}; 


if headinag(i)> 166.0, heading(istsheading(i)-36¢.0:;end: 


‘ees. eheacins: 
isstartSacple:endSample-l: 


atbsfheadingliel)-h : i ; 3 ims ie. heading i) sen 


mpsiliel)sxheading(i41)+365.0°n; 


 “eestheading. *p:71B80)=+!dos v=... 
eer Leo ys 
.*sin(heading.*pi/180;+(dop_v-... 
MBE LEO) s 


1000 10000}; 
ample) *pi/l180; 


(8) 
u_rel, heading.*pi/l@t. yaw_rate.*pi/1é8c, 


qi 
try 


variance on ur), m/s 
variance on vr} .m/s 
rariance on ucx}),.m/s 
variance on ucy),m/s 
O01; 8%tseri (variance on 2). rad/s 
on Bit. -rad?s 

On. bei)... rad 


th 2 | 
oO ooooord0 


Jen ud oe us AD pe 


Ho 


vector 
errcozeros(8,endSample!: 


Hobe 
too 


XxnX we QANDAH £& wv w 


=~ odd 
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%MANEUVERING AND CURRENT TIME CONSTANTS 





taul=1; 
tau2=1; 
$Iinitial A matrix 


A=zeros(10,10); 
X=x(1,s); Y=x(2,s); psi=x(3,s); ur=x(4,s); vr=x(5,s); ucx=x(6,s); 
ucy=x(7,8); r=x(8,s); br=x(9,s); bsi=x(10,s); 
A(1,3)=-ur*sin(psi)-vr*cos (psi); 
A(l,4)=cos(psi); 

A(1,5)=-sin(psi); 

A(1,6)=1; 

A(2,3)=ur*cos(psi)-vr*sin(psi); 
A(2,4)=sin(psi); 

A(2,5)})=cos(psi); 

A(2,7)=1; 

A(3,8)=1; 

A(4,4)=-1/taul*o; 

A(5,5)=-1/taul*o; 

A(6,6)=-1/tau2*0; 

A(7,7)=-1/tau2*0; 

A(8,9)=0; 

A(10,10}=0; 


Initial B matrix 
B=(.3;.3;0.01;q1;q2:q3;q4;q5;q6:q7)}: 
Initial C matrix 


C=zeros(8,10); 
C(1,3)=-ucx*sin(psi)+ucy*cos (psi); 
C(i 4) 1; 

C(1,6)=cos (psi); 

C(1,7)=sin{(psi); 
C(2,3)=-ucx*cos (psi) ~ucy*sin(psi); 
C(2,5)21: 

C(2,6)=-sin(psi); 

C(2,7)=cos(psi); 

C(3,4)=1; 

C(4,3)=1; 

C(4,10)=1.0; 

C5, 8)-L2 

C(5,9)=1; 

C(6,5)=1;%added for vr sensor 
C(7,1)=1;%X=lat 
C(8,2)=1;%Y=longitude 


D=zeros (8,10); 

nul=10.00; nu2=10.00; nu3=10.00; nu4=0.0100; nuS=.025000: nu6=10; nu7=10100.00; 
nu8=10100.00; 

gnu= (nul;nu2;nu3;nu4;nu5;nu6;nu7;nu8].*10; 

R=diag(gnu); % measurement noise 


Q=diag(B); tsystem noise 


Note, old_after means measured data at old time, new _before means model predicted v 
alue 


p_old_after=diag([le-1 le-1 0.1000 1.8466 1.8482 1.3789 1.3800 0.0549 0.0059 0.0601) 


f 


delx_old_after=zeros(i0,1); 


g=ones(10,1); psave=zeros(10,endSample}: 


for i=startSample: (endSample-1): 
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Rcompute linearized PHI matrix using updated aA 
okt gan pecz2c(A, Bac)? 

Qtsamseye(i9,10); 

Sohiseyve'l0.10) +A°0.125; 


repagation 
Seohi*x old after: 


ferrcer ccevariance propagation 


p-nex befsose=phi*p old atte=*pr.* 4°: 
gain calculation using linearized new C matrix and curren 


“ - 
raw ee nwPAe 
-_w ec rvarts.ances. 


fal 


e the innovation using nonlinear output propagation 
Ite new sampled data from measurements 


if yfiel 2peeyii. 2),C(L, +) =0.0*%g* enc: 

££ yliel. 3p eeyti.3).C(2,:)60.0°g’ : end; 

if yliet.é:e2y(i.4).C(03,:)=0.0%g’ ; end: 

if yield. S)wey (1, 8),C4. 2) 20.0%g"; end: 

if yliel. Sdeer(i.6),C(5.:)20.0%g’ send: 

gf tiel. Tiecey ti. 7).C(6,4 )=0:.0°o’ zend; 

if yliel Si sey(i,8).C(7,:)#0.0°%g’ ;end: 

££ y(iel. Sizsyv(i.9).C(B.:)#0.0%g’ send: 

Sif 15860052 (C7 20.0% 9" sO US, ep eo. 0*e pend: 
=p_new_befcre*c'*inv(C*p_new_before*c’ + Ri; € Kalman Gain 

peserr='{(lat(i+l)-x_new_before(1)), (long/i+1l)-x_new_before(1))]; 

State error covariance estimate update 

Cotec*s new betore*C'+R; 

relf:,iel)serr(:.i¢1).%2./diag(cpe): 

* rel(:.,ieijediag(err(:,i¢l)*inviepe)*errc(:.24h)°3; 


@ comrute gain. update toal state and error covariance 


p_old_after=[eye(10) - G°C)*p_new_before; 


psave’:,i+¢l)sdiag(p_old_after) ; 
x _new_after=x_new_before + Gterr(:,i+l1); 


xiiJisljexnew_attes: 
resetting up the linearized A matrix 


Arter 
Xexf(l,iel); Yex(2,i41); psia=x(3,i41): ur=x(4,i¢1); vrex(S,iel); 


ucxex/€, 
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i+l]}; 
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ucy=x(7,it1l); r=x(8,1+1); br=x(9,i+1); bsi=x(10,i+1); 


A(1,3)=s-ur*sin(psi) -vr*cos (psi); 
A(1,4)=cos(psi); 
A(1,5)=-sin(psi); 

A(1,6)=1; 
A(2,3)=ur*cos(psi)-vr*sin(psi); 
A(2,4)=sin(psi);: 
A(2,5)=cos(psi)}; 


treset the linearized C matrix 


C=zeros(8,10); 
C(1,3)=-ucx*sin(psi)+ucy*cos (psi); 
C(1,4)=1; 

C(1,6)=cos(psi): 

C(1,7)=sin(psi); : 
C(2,3)=-uex*cos(psi)-ucy*sin(psi) ; 
C(2,S)=1; 

C(2,6)=-sin(psi); 

C(2,.7)=cos(psi); 

C(3,4)=1; 

C(4,3)=1;:C(4,10)=1; 

C(5, 8)<1- 

C(5,9)=1; 

C({6,5)=1;%added for vr sensor 
C(7,1)2=1;%X=lat 

C(8,2)=1; tY=longitude 


end; 


est_lat(startSample)=lat(startSample) ; 
est_long(startSample)=long(startSample) ; 


for i=startSample: (endSample-1), 
est_lat(i+1)=(dop_u(i)*cos (heading(i).*pi/180) -dop_v(i)*sin(heading(i).*pi/180))* 
dt+est_lat(i); 
est_long(i+1)=(dop_u(i)*sin(heading(i) .*pi/180)+dop_v(i)*cos (heading(i).*pi/180) ) 
*dt+est_long(i); 


end; 


Xerr(startSample:endSample)=lat(startSample:endSample) -est_lat(startrSample:endSample 


Yerr (startSample:endSample)=long(startSample:endSample) -est_long(startSample:endSamp 
le)‘; 

traddr (N)=mean (abs (Xerr+j*Yerr)); 

raddr=abs (Xerr+j*Yerr)}; 


Xfiltererr(startSample:endSample)=lat(startSample:endSample)-x(1,startSample’: endSamp 
le)’; 
Yfailtererr(startSample:endSample)=long({startSample:endSample)-x(2,startSample:endSam 
ple)’; 

tradfilter(N)=mean(abs(Xfiltererr+j*yfiltererr)); 

radfilter=abs (Xfiltererr+j*yfiltererr) ; 


tend; 


$figure(l) 

$semilogx (mult, radfilter,’c’,mult,raddr,’:’) 
grid 

tylabel (‘err (m)') 
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Blabel('R mult’) 
titlel’Radial error vs increasing R’) 


£ieere th) 
plozit{startSample:endSample).x(3.startSample:endSample) .°18C/pi, ‘yx’ 
tc(starcSample:endSample),y(startSample:endSample.S) .*180/pi.‘g+’}. grid 
title(‘10 State Filtered Heading and Measured Heading’) 

xlabel(’Time (sec)’) 

ylabel(‘heading angle (degrees) ’') 


fisure!2)} 
plotficng{(starctSample:endSample),lat(startSample:endSample!.‘g--' 
x(2.startSample:endSample),x(l,startSample:endSample).‘c-.') 
hols “on 

ict{est_lons,est_lat 
xlabel(‘Ww Longtitude (m) EB} 
ylabel('S Latitude (m) N’) 
eitlefl‘ic State Filter Path with Dead Reckoning and DGPS’) 
Grid 
heed. ott 
2oce 
figurve' 3s) & Ur 
plovitistartSample:endSample).u_rel(startSample:endSample).'gx',.. 
t(startSample:endSample).x(4.startSample:endSample).’c'),grid 
title!’Relative u. x verus estimated relative u, sclid’) 
Baxis'(ES,99.0.3.1.0)) 
xlabelf‘time (sec)') 
ylabel(‘speed (m/s)') 
2507 
§errecr cist 
firéure'<) 
pictlers (6.7), ere (7,27) 
eria 
titie! ‘10 State Position Innovation Error’) 
xlabel('’errer longitude (m)') 
ylabel{i‘errer latitude {(m)°) 
figures) 
theading errers analysis for t=(622:650) 
trelative errcr for heading (compass signal / measurement} 
psihat=x(3,startSample:endSample)+x(10,startSample:endSample):; 
trer(startSample:endSanmple}); 
headhatcheading(startSample:endSarmple)'; 
pictitt.psihat.*180/pi. “ye', tt. headhac, *g+"),¢grid 
titie’'1C State Filtered Heading with Bias and Measured Headins') 
xiabel'‘time (sec}‘) 
yiabel('heading (degrees}’) 
scoe 
efilcer performance 
&c0=mean!lrel(:,startSample:endSample)’)’; 
trudcgetemean(psave(:,startSample:endSample!’)’; 
terror_budget=budget./sum(budget); 
fisure(é! 
Subolotte ts 2) 
pietit.raddr) 
gris 


xlabel(‘time (sec)') 


ylabel(‘'CR err (m)') 
title(‘Radial error for 10 state filter’) 


Program 3. MATLAB Code "ekf_fau_2.m" (cont) 


142 








APPENDIX B. STATE PROPAGATION 


Dec 6 1996 13:27 prop6.m Page 1 


function [xnew]=prop6(xold,taul, tau2, dt) 
X=xold(1);Y=xold(2) ;psi=xold(3);r=xold(4);dop_u=xold(5};dop_v=xold(6!: 


fl=dop_u*cos(psi)-dop_v*sin(psi);: 
f2=dop_u*sin{psi)+dop_v*cos(psi}: 
{3=r; 

£4=0; 

£5=0; 

f£6=0; 

fa (Els €2 eto st4 £55 26): 
xnew=xold+f.*dt:; 
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Jan 16 1997 08:58 prop9.m Page 1 
function {xmewl=prop(xeld, taul. tau2,dt) 


(2) ;Y=xold(2);psizxold(3}) :dop_ug=xold(4);:dop_vg=xeold(5) :bu=sxola(6} :kv=excld(7): 
(Ol sbsiexold($): 


(es) dss -ve“sinipsi }2 
nips: )4des vo*cosips: i); 


fe Ub WM ak oh ot oo 


Hore eDeD er eveDaisrg 


oko DD ~) HU de te AD pe 


. 
£ 
rd 
f 
£ 
f 
f 
f 
f 
£ 


r 
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‘Oct 31996 11:10 - 


function [xnew]=prop(xold,taul,tau2,dt) 


X=xold(1);Y=xold(2);psi=xold(3) :ur=xold(4) ;vr=xold(5) ;ucx=xold(6) ;ucy=xold(7); 
r=xold(8)} ;br=xold(9);bsi=xold(10}; 


fl=ur*cos (psi)-vr*sin(psi)+ucx; 
f2=ur*sin(psi)+vr*cos(psi)+ucy; 
£3=r; 

£4=0; 

€$=-1/taul*vr*d; 
£6=-1/tau2*ucx*d; 
f7=-1/tau2*ucy*d; 
£8=-1/taul*r*0; 

£9=0; £10=0; 
f=(£1;£2;£3;£4;£5;£6;£7: £8: £9; £10); 
xnew=xold+f.*dt; 
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APPENDIX C. MEASUREMENT PROPAGATION 





—.. output6.m _ 





| Dec 6 1996 13:23 
function [yhat]=outputé (xold) ; 


X=xold (1) ;¥=xold(2) ;psisxold(3) ;r=xold(4) ;ug=xold(5) ;vg=xold(6) ; 


yl=ug:; 

Y2=vg: 

y3=psi; 

y4er;yS=X;y6=Y; 
yhat=[yliy2;:y3iy4:y5:y6l; 
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Feb 5 1997 10:41 





output9.m 


functicn fyhat}jsoutput(xoldl: 


K=exole@(l):Y=xold(2) ip 
rexcid(@: :bsizxsld!9)} 
ae ene -~ as 

Ja Pe topo ale Cie ob 

Vee asS Vee be 
yarpsiebs:; 

Yorr: 

yS=%; 

ye=: 
yhatsfyl:y2Z:yliydscys 
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Program 2. MATLAB Code "output9.m” 


148 


sizxold(3):dop_ug=xold(4);dop_vg=xold(5) ;busxold(6)}:bvexcidi7); 








Oct 61996 14:15 | -- output.m 





function {yhat}=output (xold) ; 


xexold (1) ;¥=x0ld(2) :psi=xold(3) ;ur=xold(4) ;vre=xold(5) ;ucx=xold (6) ;ucy=xold(7) ; 
r=xold(8) ;br=xold(9);bsi=xold(10); 


yleurtuex*cos (psi) tucy*sin(psi); 
y2=vr-ucx*sin (psi) tucy*cos (psi); 
y3=ur; 

y4=psitbsi: 

y5=r+br; 

y6=vr; 

y7=X; 

y8=Y; 
yhat=lyliy2:y3iv4iyS:y6iy7iy8l ; 


Program 3. MATLAB Code "output.m" 
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